#include <stdarg.h>
#include <io_port.h>
#include <kprintf.h>
#include <conio.h>

#define VIDEO_START	0x0b8000

int con_offset;
uchar color = 0x02;

void gotoxy(uchar x, uchar y) 
{
    con_offset = (y * 160) + (x * 2);
}

void setcolor(uchar c) 
{
    color = c;
}

void clrscr(uchar c) 
{
    char *screen = (char *) VIDEO_START;
    while (screen < (char *) (VIDEO_START + 4000)) {
	*screen = ' ';
	*screen++;
	*screen = c;
	*screen++;
    }
}

void putchar(unsigned ch) 
{
    char *screen = (char *) VIDEO_START;
    int i;

    switch (ch) {
    case '\n':
	con_offset += 80;
	con_offset -= con_offset % 80;
	break;

    default:
	*(screen + con_offset * 2) = ch;
	*(screen + con_offset * 2 + 1) = color;
	con_offset++;
	break;
    }

    if (con_offset > 80 * 25) {
	for (i = 80 * 2; i <= 80 * 25 * 2 + 80 * 2; i++) {
	    *(screen + i - 80 * 2) = *(screen + i);
	}
	con_offset -= 80;
    }
    update_cursor(con_offset);
}

int kprintf_help(unsigned c, void **ptr) 
{
    putchar(c);

    return 0;
}

void kprintf(const char *fmt, ...)
{
    va_list args;

    va_start(args, fmt);
    (void) do_printf(fmt, args, kprintf_help, NULL);
    va_end(args);
}

void update_cursor(uint16_t position)
{
    outb(0x3D4, 0x0F);
    outb(0x3D5, (uchar) (position & 0xFF));
    outb(0x3D4, 0x0E);
    outb(0x3D5, (uchar) ((position >> 8) & 0xFF));
}
